import math

def delimeters(gear_pos):
    g1 = gear_pos[-1]
    g2 = gear_pos[0]
    
    dist = 0
    d = []
    
    for i in range(len(gear_pos)):
        gear = gear_pos[i-1] 
        to_gear = gear_pos[i]
        from_gear = gear_pos[i-2]
        (t1,t2) = cctangent(from_gear, gear)
        (t3, t4) = cctangent(gear, to_gear)
            
        if i == 0:
            th_from = math.atan2( (t1[0] - from_gear[0]) , (t1[1] - from_gear[1]) )
            d.append((0,t1[0],t1[1], th_from, from_gear[2]))
            dist += math.sqrt((t1[1] - t2[1])**2 + (t1[0] - t2[0])**2)
            
            th_ini = math.atan2( (t2[0] - gear[0]) , (t2[1] - gear[1]) )
            d.append((dist,t2[0],t2[1],th_ini,gear[2]))


        th1 = math.atan2( (t2[0] - gear[0]) , (t2[1] - gear[1]) )
        th2 = math.atan2( (t3[0] - gear[0]) , (t3[1] - gear[1]) )
        arc = gear[2]*(th1 - th2)
        dist += abs(arc)
        d.append((dist, t3[0], t3[1], th1, gear[2]))
                   #total distance, x, y, initial angle, radius
        
        length = math.sqrt((t4[1] - t3[1])**2 + (t4[0] - t3[0])**2)
        dist += abs(length)
        d.append((dist, t4[0], t4[1], 0, 0, 0))
                   #total distance, x, y               

    delimeters = d
    for delim in d:
        print delim[0], '(', delim[1], ',', delim[2], ')'
    return d

def cctangent(c1, c2):
    c1x=c1[0]
    c1y=c1[1]
    c1r=c1[2]
    c2x=c2[0]
    c2y=c2[1]
    c2r=c2[2]
    th_ini = math.atan2(c2y-c1y,c2x-c1x)
    l = math.sqrt((c2y-c1y)**2 + (c2x-c1x)**2)
    th = math.acos((c1r-c2r)/l)
    t1x = c1x + c1r * math.cos(th+th_ini)
    t1y = c1y + c1r * math.sin(th+th_ini) 
    t2x = c2x + c2r * math.cos(th+th_ini)
    t2y = c2y + c2r * math.sin(th+th_ini)
    return ((t1x, t1y), (t2x, t2y))




def get_xy_pos(d, dlist):
#        d = self.rate*time - dist_behind
    for i in range(len(dlist)-1):
        #delimeters in the form [(d1, x1, y1, th_ini, r), (d2, x2, y2)]
        p1 = dlist[i]
        p2 = dlist[i+1]
        if d >= dlist[-2][0]:      #The front creep has done a lap
            return x, y
        if p1[0] <= d < p2[0]:
            print p1[0], '(', p1[1], ',', p1[2], ')'
            if i % 2 == 0:      #even sections are linear
                x = p1[1] + (d - p1[0]) / (p2[0] - p1[0]) * (p2[1] - p1[1])
                y = p1[2] + (d - p1[0]) / (p2[0] - p1[0]) * (p2[2] - p1[2])
            else:               #odd sections are turns
                x = p1[1] + math.cos(p1[3] + (d - p1[0])/p1[4])
                y = p1[1] + math.sin(p1[3] + (d - p1[0])/p1[4])
            return x, y

print get_xy_pos (5, delimeters([(2,2,-1), (-2,2, -1), (-2,-2,-1), (2,-2,-1)]) )
